#include "pid.h"

const float motorSpeed = 80; //
const int IR_PIN[] = {A1, A2, A3, A4, A5}; //OUT1~OUT5
const int _pwmLeftPin = 5;
const int _pwmRightPin = 6;
pid_t pid;
float pidValue = 0;
bool turnFlag = false;

void pid_init(void)
{
  int i;
  for (i = 0; i < 5; i++) 
  {
    pinMode(IR_PIN[i], INPUT);
  }

  pid.sampleTime = SAMPLE_TIME;
  pid.Kp = KP_VALUE;
  pid.Ki = KI_VALUE;
  pid.Kd = KD_VALUE;
  pid.error = 0;
  pid.previous_error = 0;

  analogWrite(_pwmLeftPin,  motorSpeed);
  analogWrite(_pwmRightPin, motorSpeed);

  car_forward();
  return;
}


//get ir data and middle filter
uint8_t getIrData(void)  
{
  int i = 0, j = 0;
  uint8_t level = 0;
  uint8_t temp = 0;
  //采集9次传感器的传感器的数据，减少误差，取平均值，滤波。
  uint8_t irs[9] = {0};

  for (j = 0; j < 9; j++) 
  {
    for (i = 0; i < 5; i++) 
    {
      level = digitalRead(IR_PIN[i]);
      if (level) 
      {
        bitSet(irs[j], i); // 设置irs[j]的第jbit 为1
      } 
      else 
      {
        bitClear(irs[j], i); //清除irs[j]的第jbit为0
      }
    }
  }

  //对irs中的数据，进行9次排序，取最中间的值。
  for (i = 0; i < 9 - 1; i++) 
  {
    for (j = 0; j < 9 - i - 1; j++) 
    {
      if (irs[j] > irs[j + 1]) 
      {
        temp = irs[j];
        irs[j] = irs[j + 1];
        irs[j + 1] = temp;
      }
    }
  }
  
  return irs[4];
}

int calcErrorByIrsValue(uint8_t irs)
{
  //获得上一次的pid误差值
  int curError = pid.error; 

  //压线输出低电平0，没有压线输出高电平1     [五路算法]
  switch (irs) 
  { 
    case B11110: curError = -8; break;  
    
    case B10000:
    case B11000: curError = -7; break;
    
    case B11100: curError = -6; break; 
    case B11101: curError = -4; break; 
    case B11001: curError = -2; break; 
    
    case B00000:				 
    case B11011: curError = 0;  break;
    
    case B10011: curError = 2;  break;
    case B10111: curError = 4;  break;
    case B00111: curError = 6;  break;
    
    case B00011:
    case B00001: curError = 7;  break;
    
    case B01111: curError = 8;  break;
	//都没有沿线，按照上次的PID值来计算
    case B11111: curError = pid.error > 0 ? 9 : - 9; break;
  }

  return curError;
}



void _sortData(int *p, int n)
{
  int temp;
  int i, j;
  
  for (i = 0; i < n - 1; i ++) 
  {
    for (j = 0; j < n - i - 1; j ++) 
    {
      if (p[j] > p[j + 1]) 
      {
        temp = p[j];
        p[j] = p[j + 1];
        p[j + 1] = temp;
      }
    }
  }

  return ;
}


//计算当前的误差值
void calcCurrentError(void)
{
  int i;
  uint8_t irs;
  float sum = 0;
  int errorData[10];

  // 采样误差10次
  for (i = 0; i < 10; i ++) 
  {
    //得到这次传感器采集的数据（5路数据）
    irs =  getIrData();  
    errorData[i] = calcErrorByIrsValue(irs);
  }

  //排序    0，-2，2，3，-8，8，-2，0
  _sortData(errorData, 10);  

  //去掉一个最低值，去掉一个最高值
  for (i = 1; i < 10 - 1; i ++) 
  { 
    sum += errorData[i];
  }

  // 最后平均分
  pid.error = sum / 8;

  return ;
}


void motorControl(float pidValue, bool turnFlag)
{
  int leftMotorSpeed = 0;
  int rightMotorSpeed = 0;
  

  leftMotorSpeed  = constrain((motorSpeed + pidValue), -255, 255);
  rightMotorSpeed = constrain((motorSpeed - pidValue), -255, 255);

  if (turnFlag) 
  {
    if (abs(leftMotorSpeed) > abs(rightMotorSpeed)) 
    {
      leftMotorSpeed  = abs(leftMotorSpeed);
      rightMotorSpeed = leftMotorSpeed;
    } 
    else
    {
      rightMotorSpeed =  abs(rightMotorSpeed);
      leftMotorSpeed  = rightMotorSpeed;
    }
  } 
  else 
  {
    leftMotorSpeed  = leftMotorSpeed  > 0 ? leftMotorSpeed  : -leftMotorSpeed;
    rightMotorSpeed = rightMotorSpeed > 0 ? rightMotorSpeed : -rightMotorSpeed;
  }

  analogWrite(_pwmLeftPin,  leftMotorSpeed );
  analogWrite(_pwmRightPin, rightMotorSpeed);

  return;
}

bool calculatePid(float *pValue)
{
  float P = 0;
  static float I = 0 ;
  float D = 0 ;
  static unsigned long lastTime = 0;
  //获得机器从启动到现在所运行的时间
  unsigned long now = millis(); 
  //得到这次的时间
  int timeChange = now - lastTime;//得到这次的时间

  //若是我们的这次变化的时间，小于pid的采样时间，就需要等待一下。
  if (timeChange < pid.sampleTime) 
  {
    return false;
  }

  //本次小车传感器的偏移大小
  P = pid.error;
  //历史的偏移
  I = I + pid.error;
  //最近两次的差值
  D = pid.error - pid.previous_error; 

  *pValue = (pid.Kp * P) + (pid.Ki * I) + (pid.Kd * D) + 1;
  //constrain()函数的功能：若是*pvalue > motorSpeed,取 motorSpeed，
  //                       若是*pvalue < -motorSpeed,取 -motorSpeed,
  //					   若是-pValue<=*pvalue <= motorSpeed,取*pvalue
  *pValue = constrain(*pValue, -motorSpeed, motorSpeed);

  //记录本次的pid.error，作为下一次的历史差值。
  pid.previous_error = pid.error; 
  //记录本次的运行时间，作为下一次时间的历史时间参考。
  lastTime = now; 

  return true;
}

#if ARDUINO_DEBUG
void print_debug()
{
  int i;
  String irs2bin = String(debugIrs, 2);
  int len = irs2bin.length();
  if (len < 5) {
    for (i = 0; i < 5 - len; i++) {
      irs2bin = "0" + irs2bin;
    }
  }
  
  Serial.print("IRS : ");
  Serial.print(irs2bin);
  Serial.print("   ML:");
  Serial.print(debugLeftSpeed);
  Serial.print("   MR:");
  Serial.print(debugRightSpeed);
  Serial.print("  ERROR:");
  Serial.print(pid.error, OCT);
  Serial.println();
}
#endif

//判断小车的转向
void calcDirection(void) 
{
  if (pid.error >= 7 && pid.error <= 8) 
  {
    car_left();
    turnFlag = true;
  } 
  else if (pid.error >= -8 && pid.error <= -7) 
  {
    car_right();
    turnFlag = true;
  } 
  else if(pid.error == 9 || pid.error == -9)
  {
    car_right();
    turnFlag = true;
  }
  else
  {
    car_forward();
    turnFlag = false;
  }

  return ;
}


void pid_test(void)
{
  bool ok = true;
  float  pidValue = 0.0;

  // 计算当前偏差值
  calcCurrentError();
  ok = calculatePid(&pidValue); 
  if (ok) 
  {
    calcDirection();
    motorControl(pidValue, turnFlag);
  }

#if ARDUINO_DEBUG
  print_debug();
  delay(1000);
#endif 

/*
goForward(); 
delay(500);
turnRight();
delay(500);
turnLeft();
delay(500);
 */
  return;
}

